#coding:utf-8
import numpy as np
import copy

def distance(position_1, position_2):
    return np.abs(position_1[0]-position_2[0]) + np.abs(position_1[1] + position_2[1])

def select_shortest_position(positions, target):
    minium = 0
    for position in positions:
        temp = distance(position, target)
        if minium > temp:
            minium = temp
            select_position = position
    return select_position
    pass


# -*- coding: utf-8 -*-
import math

# 地图
tm = [
    '############################################################',
    '#..........................................................#',
    '#.............................#............................#',
    '#.............................#............................#',
    '#.............................#............................#',
    '#.......S.....................#............................#',
    '#.............................#............................#',
    '#.............................#............................#',
    '#.............................#............................#',
    '#.............................#............................#',
    '#.............................#............................#',
    '#.............................#............................#',
    '#.............................#............................#',
    '#######.#######################################............#',
    '#....#........#............................................#',
    '#....#........#............................................#',
    '#....##########............................................#',
    '#..........................................................#',
    '#..........................................................#',
    '#..........................................................#',
    '#..........................................................#',
    '#..........................................................#',
    '#...............................##############.............#',
    '#...............................#............#.............#',
    '#...............................#..E.........#.............#',
    '#...............................#............#.............#',
    '#...............................#............#.............#',
    '#...............................###########..#.............#',
    '#..........................................................#',
    '#..........................................................#',
    '############################################################']

# 因为python里string不能直接改变某一元素，所以用test_map来存储搜索时的地图
test_map = []
mp_wight = 12
mp_height = 12
constrct_map = []
#########################################################
class Node_Elem:
    """
    开放列表和关闭列表的元素类型，parent用来在成功的时候回溯路径
    """

    def __init__(self, parent, x, y, dist):
        self.parent = parent
        self.x = x
        self.y = y
        self.dist = dist


class A_Star:
    """
    A星算法实现类
    """

    # 注意w,h两个参数，如果你修改了地图，需要传入一个正确值或者修改这里的默认参数
    def __init__(self, s_x, s_y, e_x, e_y, w=mp_wight, h=mp_height):
        self.s_x = s_x
        self.s_y = s_y
        self.e_x = e_x
        self.e_y = e_y

        self.width = w
        self.height = h

        self.open = []
        self.close = []
        self.path = []

    # 查找路径的入口函数
    def find_path(self):
        # 构建开始节点
        p = Node_Elem(None, self.s_x, self.s_y, 0.0)
        while True:
            # 扩展F值最小的节点
            self.extend_round(p)
            # 如果开放列表为空，则不存在路径，返回
            if not self.open:
                return
            # 获取F值最小的节点
            idx, p = self.get_best()
            # 找到路径，生成路径，返回
            if self.is_target(p):
                self.make_path(p)
                return
            # 把此节点压入关闭列表，并从开放列表里删除
            self.close.append(p)
            del self.open[idx]

    def make_path(self, p):
        # 从结束点回溯到开始点，开始点的parent == None
        while p:
            self.path.append((p.x, p.y))
            p = p.parent

    def is_target(self, i):
        return i.x == self.e_x and i.y == self.e_y

    def get_best(self):
        best = None
        bv = 1000000  # 如果你修改的地图很大，可能需要修改这个值
        bi = -1
        for idx, i in enumerate(self.open):
            value = self.get_dist(i)  # 获取F值
            if value < bv:  # 比以前的更好，即F值更小
                best = i
                bv = value
                bi = idx
        return bi, best

    def get_dist(self, i):
        # F = G + H
        # G 为已经走过的路径长度， H为估计还要走多远
        # 这个公式就是A*算法的精华了。
        # return i.dist + math.sqrt(
        #     (self.e_x - i.x) * (self.e_x - i.x)
        #     + (self.e_y - i.y) * (self.e_y - i.y)) * 1.2
        return i.dist + (np.abs((self.e_x - i.x)) + np.abs((self.e_y - i.y)))

    def extend_round(self, p):
        # 可以从8个方向走
        # xs = (-1, 0, 1, -1, 1, -1, 0, 1)
        # ys = (-1, -1, -1, 0, 0, 1, 1, 1)
        # 只能走上下左右四个方向
        xs = (0, -1, 1, 0)
        ys = (-1, 0, 0, 1)
        for x, y in zip(xs, ys):
            new_x, new_y = x + p.x, y + p.y
            # 无效或者不可行走区域，则勿略
            if not self.is_valid_coord(new_x, new_y):
                continue
            # 构造新的节点
            node = Node_Elem(p, new_x, new_y, p.dist + self.get_cost(
                p.x, p.y, new_x, new_y))
            # 新节点在关闭列表，则忽略
            if self.node_in_close(node):
                continue
            i = self.node_in_open(node)
            if i != -1:
                # 新节点在开放列表
                if self.open[i].dist > node.dist:
                    # 现在的路径到比以前到这个节点的路径更好~
                    # 则使用现在的路径
                    self.open[i].parent = p
                    self.open[i].dist = node.dist
                continue
            self.open.append(node)

    def get_cost(self, x1, y1, x2, y2):
        """
        上下左右直走，代价为1.0，斜走，代价为1.4
        """
        if x1 == x2 or y1 == y2:
            return 1.0
        return 1.4

    def node_in_close(self, node):
        for i in self.close:
            if node.x == i.x and node.y == i.y:
                return True
        return False

    def node_in_open(self, node):
        for i, n in enumerate(self.open):
            if node.x == n.x and node.y == n.y:
                return i
        return -1

    def is_valid_coord(self, x, y):
        if x < 0 or x >= self.width or y < 0 or y >= self.height:
            return False
        return test_map[x][y] != '#'

    def get_searched(self):
        l = []
        for i in self.open:
            l.append((i.x, i.y))
        for i in self.close:
            l.append((i.x, i.y))
        return l


#########################################################
def print_test_map():
    """
    打印搜索后的地图
    """
    for line in test_map:
        # print (''.join(line))
        pass


def get_start_XY():
    return get_symbol_XY('S')

def set_start_XY(x, y):
    constrct_map[x][y] = 'S'

def reset_map(x, y):
    constrct_map[x][y] = '*'

def get_end_XY():
    return get_symbol_XY('E')

def set_end_XY(x, y):
    constrct_map[x][y] = 'E'

def get_symbol_XY(s):
    for x, line in enumerate(test_map):
        if x == 11:
            pass
            pass
        try:
            y = line.index(s)
        except:
            continue
        else:
            break
    try:
        return x, y
    except UnboundLocalError:
        pass

def set_occupied(occupied):
    for w in range(mp_wight):
        constrct_map[w][0] = '#'
        constrct_map[w][-1] = '#'
    for h in range(mp_height):
        constrct_map[0][h] = '#'
        constrct_map[-1][h] = '#'
    for pair in occupied:
        constrct_map[pair[0]][pair[1]] = "#"
    pass

#########################################################
def mark_path(l):
    mark_symbol(l, '*')


def mark_searched(l):
    mark_symbol(l, ' ')


def mark_symbol(l, s):
    for x, y in l:
        test_map[x][y] = s


def mark_start_end(s_x, s_y, e_x, e_y):
    test_map[s_x][s_y] = 'S'
    test_map[e_x][e_y] = 'E'


def tm_to_test_map(tm):
    for line in tm:
        test_map.append(list(line))


def find_path():
    s_x, s_y = get_start_XY()
    e_x, e_y = get_end_XY()
    a_star = A_Star(s_x, s_y, e_x, e_y)
    a_star.find_path()
    searched = a_star.get_searched()
    path = a_star.path
    for p in path:
        # print(' == >> path %s ' % list(p))
        pass
    # 标记已搜索区域
    mark_searched(searched)
    # 标记路径
    mark_path(path)
    # print("path length is %d" % (len(path)))
    # print("searched squares count is %d" % (len(searched)))
    # 标记开始、结束点
    mark_start_end(s_x, s_y, e_x, e_y)
    return path


def constructMAP(source, target, occupied):
    for w in range(mp_wight):
        temp = []
        for h in range(mp_height):
            temp.append('*')
        constrct_map.append(temp)
    set_occupied(occupied)
    set_start_XY(source[0], source[1])
    set_end_XY(target[0], target[1])
    for line in constrct_map:
        # print(''.join(line))
        pass
    pass

def axischange(position, flag):
    if flag:
        # position[0] = position[0] - 1
        # position[1] = position[1] - 1
        return [position[0] - 1, position[1] - 1]
    else:
        try:
            # position[0] = position[0] + 1
            # position[1] = position[1] + 1
            return [position[0] + 1, position[1] + 1]
        except IndexError:
            pass
def check_occupied_source_target(src, occ):
    return src[0]==occ[0] and src[1]==occ[1]

def check_state(source, target, occupied, state, flag):
    '''
    对 occupied 进行坐标转换
    :param source:
    :param target:
    :param occupied:
    :param state:
    :param flag:
    :return:
    '''
    for item in [source, target]:
        if item in occupied:
            occupied.remove(item)

    if state == 'in':
        for pair in occupied:
            axischange(pair, flag)
    if state == 'reset':
        for pair in occupied:
            if check_occupied_source_target(pair, source) or check_occupied_source_target(pair, target):
                pass
            else:
                axischange(pair, flag)
    if state == 'remove':
        for pair in occupied:
            axischange(pair, flag)

def change_state(source, target, occupied, state, flag):
    # 转换 occupied
    # if state == 'remove':
    #     for pair in occupied:
    #         axischange(pair, flag)
    # elif state == 'in':
    #     for pair in occupied:
    #         if check_occupied_source_target(pair, source) or check_occupied_source_target(pair, target):
    #             pass
    #         else:
    #             axischange(pair, flag)
    # elif state == 'reset':
    #     for pair in occupied:
    #         if check_occupied_source_target(pair, source) or check_occupied_source_target(pair, target):
    #             pass
    #         else:
    #             axischange(pair, flag)
    changed_occupied = []
    if flag:
        for item in occupied:
            changed_occupied.append([item[0] - 1, item[1] - 1])
    else:
        for item in occupied:
            changed_occupied.append([item[0] + 1, item[1] + 1])
    # 转换 socure 和 target
    source = axischange(source, flag)
    target = axischange(target, flag)

    return source, target, changed_occupied

def select_path(source_, target_, occ_, state):
    '''
    选路坐标是从 [1, 1] 开始
    :param source: 开始位置 copied
    :param target: 终止位置 copied
    :param occ: 被占有的部分
    :param state: 控制坐标转换 false: [pos]+1, true: [pos]-1
    :return: 轨迹信息
    '''
    # 将 source 和 target 从 occ 中分离出来
    source = copy.copy(source_)
    target = copy.copy(target_)
    occ = copy.copy(occ_)

    if source == target:
        print("source is the same as target!")
        return False
    if source == '' or target == '':
        print("source or target is None")
        return False

    source, target, occ = change_state(source, target, occ, state, flag=False)

    global constrct_map
    constrct_map = []
    # 把字符串转成列表
    if not source or not target:
        print("source or target is None")
    constructMAP(source, target, occ)
    # tm_to_test_map(tm)
    global test_map
    test_map = constrct_map
    path = find_path()
    if len(path) == 0:
        # change_state(source, target, occ, state, True)
        print('path length is zero.')
        return False
    print_test_map()
    path.reverse()
    result = []
    for item in path:
        result.append([item[0]-1, item[1]-1])
    # check_state(source, target, occ, state, True)
    return result

# if __name__ == "__main__":
#     source = [6, 1]
#     target = [2, 3]
#     # occupied = [[3, 3], [3, 2], [4, 4], [4, 5], [4, 6], [4, 7], [4, 8], [4, 9]]
#     occupied = [[5, 1], [5, 2], [5, 3], [3, 3]]
#     select_path(source, target, occupied)
